import os
import sys
import termios
import fcntl
import threading
import time
from enum import Enum
import struct
import queue

# 接收结构体
class Receive_Data_C:
    def __init__(self):
        self.dart_status = -1
        self.pitch_angle = 0
        self.yaw_angle = 0
        self.roll_angle = 0
        self.temperature = 0
    def my_print(self):
        print(f"dart_status:{self.dart_status},pitch_angle:{self.pitch_angle},yaw_angle:{self.yaw_angle},roll_angle:{self.roll_angle},temperature:{self.temperature}")
        #print("1")
        
    def data_arrange(self,data):
        self.dart_status = data[0]
        self.pitch_angle = struct.unpack('f', data[1:5])[0]
        self.yaw_angle = struct.unpack('f', data[5:9])[0]
        self.roll_angle = struct.unpack('f', data[9:13])[0]
        self.temperature = struct.unpack('f', data[13:17])[0]

class SerialProtocolError(Exception):
    """Base exception for serial protocol errors"""
    pass

class SerialOpenError(SerialProtocolError):
    """Exception raised when serial port cannot be opened"""
    pass

class SerialProtocolHandler:
    '''用于装帧帧和处理数据的串行协议处理句柄'''
    # 协议相关常量
    MYPACK_FRAMEHEADER_SOF1 = 0xA5
    MYPACK_FRAMEHEADER_SOF2 = 0xA6
    BUFFER_SIZE = 256
    
    # CRC校验表
    wCRC_Table = [
        0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
        0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
        0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
        0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
        0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
        0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
        0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
        0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
        0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
        0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
        0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
        0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
        0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
        0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
        0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
        0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
        0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
        0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
        0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
        0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
        0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
        0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
        0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
        0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
        0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
        0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
        0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
        0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
        0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
        0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
        0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
        0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
    ]
    class PACKRX_STATUS_ENUM(Enum):
        '''接收状态机状态'''
        MyPack_Frame_SOF = 0
        MyPack_Frame_DataLength = 1
        MyPack_Frame_DATA = 2
        MyPack_Frame_CRC16 = 3

    class RXBuffer:
        '''接收缓冲区'''
        def __init__(self):
            self.length = 0
            self.buffer = bytearray(SerialProtocolHandler.BUFFER_SIZE)
            self.all_data = bytearray(SerialProtocolHandler.BUFFER_SIZE + 5)
            self.CRC_data = 0
            self.CRC_success = False
            self.bag_count = 0

    class Unpacker:
        '''解包器'''
        def __init__(self):
            self.PackRX_Status = SerialProtocolHandler.PACKRX_STATUS_ENUM.MyPack_Frame_SOF.value
            self.SOF_count = 0
            self.DataLenth_count = 0
            self.CRC16_count = 0
            self.RX_Buffer = SerialProtocolHandler.RXBuffer()

    def __init__(self, port='/dev/serial0', baudrate=115200,data_bits=8,stop_bits=1,parity='N',callback=None):
        # 两个进程的处理时间
        self.process_waittime = 0.002
        self.receive_waittime = 0.002
        self.port = port
        self.baudrate = baudrate
        self.data_bits = data_bits
        self.stop_bits = stop_bits
        self.parity = parity
        self.callback = callback
        self.fd = None
        self.mtx = threading.Lock()
        self.receive_queue = queue.Queue()
        self.buffer_shared = bytearray(self.BUFFER_SIZE)
        self.receive_finished = False
        self.unpacker = self.Unpacker()
        self._running = False
        self._configure_serial_port()
        
        
    def _configure_serial_port(self):
        """Initialize and configure serial port 
        termios.tcgetattr返回的是一个列表,其中各个元素的顺序为:[iflag, oflag, cflag, lflag, ispeed, ospeed, cc]
        """
        try:
            self.fd = os.open(self.port, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK)
        except OSError as e:
            raise SerialOpenError(f"Unable to open {self.port}: {e}") from e
        # 波特率映射表
        baud_rate_mapping = {
            2400: termios.B2400,
            4800: termios.B4800,
            9600: termios.B9600,
            19200: termios.B19200,
            38400: termios.B38400,
            57600: termios.B57600,
            115200: termios.B115200,
            230400: termios.B230400,
            460800: termios.B460800,
            500000: termios.B500000,
            576000: termios.B576000,
            921600: termios.B921600,
            1000000: termios.B1000000,
            1152000: termios.B1152000,
            1500000: termios.B1500000,
            2000000: termios.B2000000,
            2500000: termios.B2500000,
            3000000: termios.B3000000,
            3500000: termios.B3500000,
            4000000: termios.B4000000,
        }

        try:
            # 获取当前串口属性
            attrs = termios.tcgetattr(self.fd)
        except termios.error as e:
            print(f"Error getting serial port attributes: {e}", file=sys.stderr)
            return

        # 处理输入模式标志（禁用特殊字符处理）
        attrs[0] &= ~(termios.ICRNL | termios.INLCR | termios.IGNCR)

        # 设置波特率
        if self.baudrate not in baud_rate_mapping:
            print(f"Invalid baud rate: {self.baudrate}", file=sys.stderr)
            return
        speed = baud_rate_mapping[self.baudrate]
        attrs[4] = speed  # 输入速度
        attrs[5] = speed  # 输出速度

        # 设置数据位
        attrs[2] &= ~termios.CSIZE
        if self.data_bits == 5:
            attrs[2] |= termios.CS5
        elif self.data_bits == 6:
            attrs[2] |= termios.CS6
        elif self.data_bits == 7:
            attrs[2] |= termios.CS7
        elif self.data_bits == 8:
            attrs[2] |= termios.CS8
        else:
            print("Invalid data bits. Must be 5, 6, 7, or 8.", file=sys.stderr)
            return

        # 设置停止位
        if self.stop_bits == 1:
            attrs[2] &= ~termios.CSTOPB
        elif self.stop_bits == 2:
            attrs[2] |= termios.CSTOPB
        else:
            print("Invalid stop bits. Must be 1 or 2.", file=sys.stderr)
            return

        # 设置校验位 (大小写均可)
        parity = self.parity.upper()
        if parity == 'N':
            attrs[2] &= ~termios.PARENB   # 关闭奇偶校验
            attrs[0] &= ~termios.INPCK    # 禁用输入校验
        elif parity == 'O':
            attrs[2] |= termios.PARENB    # 启用奇偶校验
            attrs[2] |= termios.PARODD    # 奇校验
            attrs[0] |= termios.INPCK     # 启用输入校验
        elif parity == 'E':
            attrs[2] |= termios.PARENB    # 启用奇偶校验
            attrs[2] &= ~termios.PARODD   # 偶校验
            attrs[0] |= termios.INPCK     # 启用输入校验
        else:
            print("Invalid parity option. Must be 'N', 'O', or 'E'.", file=sys.stderr)
            return

        # 配置基础控制模式
        attrs[2] |= termios.CLOCAL | termios.CREAD  # 忽略调制解调器状态，启用接收
        attrs[2] &= ~termios.CRTSCTS                # 禁用硬件流控

        # 配置输入模式
        attrs[0] &= ~(termios.IXON | termios.IXOFF | termios.IXANY)  # 禁用软件流控

        # 配置为原始模式
        attrs[3] &= ~(termios.ICANON | termios.ECHO | termios.ECHOE | termios.ISIG)  # 非规范模式
        attrs[1] &= ~termios.OPOST  # 原始输出

        # 配置超时和最小字符数
        attrs[6][termios.VMIN] = 0   # 非阻塞模式
        attrs[6][termios.VTIME] = 0  # 立即返回

        try:
            # 应用配置
            termios.tcsetattr(self.fd, termios.TCSANOW, attrs)
        except termios.error as e:
            print(f"Error setting serial port attributes: {e}", file=sys.stderr)
            return

        termios.tcflush(self.fd, termios.TCIFLUSH)
        termios.tcflush(self.fd, termios.TCOFLUSH)




    def start(self):
        """Start the receiver and processor threads"""
        self._running = True
        receiver = threading.Thread(target=self._receive_thread)
        processor = threading.Thread(target=self._process_thread)
        receiver.daemon = True
        processor.daemon = True
        receiver.start()
        processor.start()
    def start_quene(self):
        """Start the receiver and processor threads"""
        self._running = True
        receiver = threading.Thread(target=self._receive_thread)
        processor = threading.Thread(target=self._process_quene_thread)
        receiver.daemon = True
        processor.daemon = True
        receiver.start()
        processor.start()

    def stop(self):
        """Stop threads and close serial port"""
        self._running = False
        if self.fd:
            os.close(self.fd)
            self.fd = None

    def _receive_thread(self):
        """Thread for receiving and unpacking data"""
        while self._running:
            try:
                data = os.read(self.fd, 256)
                if data:
                    for byte in data:
                        self._unpack_byte(byte)
            except BlockingIOError:
                pass
            except OSError as e:
                if self._running:
                    raise SerialProtocolError(f"Serial read error: {e}")
            time.sleep(self.receive_waittime)

    def _unpack_byte(self, byte):
        """Process received bytes through unpacking state machine"""
        rx = self.unpacker.RX_Buffer
        status = self.unpacker.PackRX_Status
        
        if status == self.PACKRX_STATUS_ENUM.MyPack_Frame_SOF.value:
            if byte == self.MYPACK_FRAMEHEADER_SOF1 and self.unpacker.SOF_count == 0:
                rx.all_data[0] = byte
                self.unpacker.SOF_count = 1
            elif byte == self.MYPACK_FRAMEHEADER_SOF2 and self.unpacker.SOF_count == 1:
                rx.all_data[1] = byte
                self.unpacker.SOF_count = 0
                rx.length = 0
                self.unpacker.PackRX_Status = self.PACKRX_STATUS_ENUM.MyPack_Frame_DataLength.value
            else:
                self.unpacker.SOF_count = 0

        elif status == self.PACKRX_STATUS_ENUM.MyPack_Frame_DataLength.value:
            rx.length = byte
            rx.all_data[2] = byte
            self.unpacker.PackRX_Status = self.PACKRX_STATUS_ENUM.MyPack_Frame_DATA.value
            self.unpacker.DataLenth_count = 0

        elif status == self.PACKRX_STATUS_ENUM.MyPack_Frame_DATA.value:
            if self.unpacker.DataLenth_count < self.BUFFER_SIZE:
                rx.buffer[self.unpacker.DataLenth_count] = byte
                rx.all_data[3 + self.unpacker.DataLenth_count] = byte
                self.unpacker.DataLenth_count += 1
                if self.unpacker.DataLenth_count >= rx.length:
                    self.unpacker.CRC16_count = 0
                    self.unpacker.PackRX_Status = self.PACKRX_STATUS_ENUM.MyPack_Frame_CRC16.value

        elif status == self.PACKRX_STATUS_ENUM.MyPack_Frame_CRC16.value:
            if self.unpacker.CRC16_count == 0:
                rx.CRC_data = byte
                rx.all_data[3 + rx.length] = byte
                self.unpacker.CRC16_count = 1
            elif self.unpacker.CRC16_count == 1:
                rx.all_data[4 + rx.length] = byte
                rx.CRC_data |= byte << 8
                self.unpacker.PackRX_Status = self.PACKRX_STATUS_ENUM.MyPack_Frame_SOF.value
                if self._verify_crc(rx.all_data, rx.length + 5):
                    # 队列接收
                    self.receive_queue.put(rx.buffer[:rx.length])
                    with self.mtx:
                        self.buffer_shared[:rx.length] = rx.buffer[:rx.length]
                        self.receive_finished = True
                        rx.bag_count += 1
                        print(f"receivebagcount:{rx.bag_count}")

    def _verify_crc(self, data, length):
        """Verify CRC checksum for received data"""
        if length <= 2:
            return False
        wExpected = self._get_crc16(data[:length-2], 0xFFFF)
        return (wExpected & 0xFF == data[length-2] and 
                (wExpected >> 8) & 0xFF == data[length-1])

    @classmethod
    def _get_crc16(cls, data, crc_init):
        """Calculate CRC16 checksum"""
        crc = crc_init
        for byte in data:
            crc = (crc >> 8) ^ cls.wCRC_Table[(crc ^ byte) & 0xFF]
        return crc

    def _process_thread(self):
        """Thread for processing received data"""
        while self._running:
            with self.mtx:
                if self.receive_finished:
                    self.receive_finished = False
                    data = bytes(self.buffer_shared[:self.unpacker.RX_Buffer.length])
                    
                    if self.callback:
                        try:
                            response = self.callback(data)
                            if response:
                                self.send(response)
                        except Exception as e:
                            print(f"Callback error: {e}")
            time.sleep(self.process_waittime)

    def _process_quene_thread(self):
        while self._running:
            data = bytes(self.receive_queue.get())
            if self.callback:
                try:
                    response = self.callback(data)
                    if response:
                        self.send(response)
                except Exception as e:
                    print(f"Callback error: {e}")

    def send(self, data):
        """Send data through serial port with protocol framing"""
        if not self.fd:
            raise SerialProtocolError("Serial port not open")
        
        message = bytearray([self.MYPACK_FRAMEHEADER_SOF1,
                            self.MYPACK_FRAMEHEADER_SOF2,
                            len(data)])
        message.extend(data)
        message.extend(b'\x00\x00')  # CRC placeholder
        
        # Calculate and append CRC
        crc = self._get_crc16(message[:-2], 0xFFFF)
        message[-2] = crc & 0xFF
        message[-1] = (crc >> 8) & 0xFF
        
        try:
            os.write(self.fd, message)
        except OSError as e:
            raise SerialProtocolError(f"Serial write error: {e}") from e

# 使用示例
if __name__ == '__main__':
    def example_callback(data):
        print(f"Received: {data.decode()}")
        return b"ACK"
    
    try:
        handler = SerialProtocolHandler(port='/dev/serial0', callback=example_callback)
        handler.start()
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        handler.stop()